// Copyright (c) 2024, lfr
// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

//
// Source of this file are templates in
// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
//

#ifndef LQR_VMC_CONTROLLER__LQR_VMC_CONTROLLER_HPP_
#define LQR_VMC_CONTROLLER__LQR_VMC_CONTROLLER_HPP_

#include <chrono>
#include <memory>
#include <string>
#include <vector>

#include "controller_interface/controller_interface.hpp"
#include "lqr_vmc_controller/visibility_control.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"
#include "std_srvs/srv/set_bool.hpp"

#include "hardware_interface/types/hardware_interface_type_values.hpp"

// TODO(anyone): Replace with controller specific messages
#include "control_msgs/msg/joint_controller_state.hpp"
#include "control_msgs/msg/joint_jog.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "semantic_components/imu_sensor.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "lqr_vmc_controller/odometry.hpp"
#include "odometry.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "lqr_vmc.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "realtime_tools/realtime_box.h"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "debug_msgs/msg/debug_data.hpp"
#include "std_msgs/msg/bool.hpp"
#include "fivethplanner.hpp"

namespace lqr_vmc_controller
{
// name constants for state interfaces
// static constexpr size_t STATE_MY_ITFS = 0;
static constexpr size_t right_thigh_link_joint_position = 0;
static constexpr size_t right_thigh_link_joint_velocity = 1;
static constexpr size_t right_thigh_link_joint_effort_feedback = 2;
static constexpr size_t left_thigh_link_joint_position = 3;
static constexpr size_t left_thigh_link_joint_velocity = 4;
static constexpr size_t left_thigh_link_joint_effort_feedback = 5;
static constexpr size_t right_wheel_link_joint_position = 6;
static constexpr size_t right_wheel_link_joint_velocity = 7;
static constexpr size_t right_wheel_link_joiht_effort_feedback = 8;
static constexpr size_t left_wheel_link_joint_position = 9;
static constexpr size_t left_wheel_link_joint_velocity = 10;
static constexpr size_t left_wheel_link_joint_effort_feedback = 11;

// name constants for command interfaces
// static constexpr size_t CMD_MY_ITFS = 0;
static constexpr size_t right_thigh_link_joint_effort = 0;
static constexpr size_t left_thigh_link_joint_effort = 1;
static constexpr size_t right_wheel_link_joint_effort = 2;
static constexpr size_t left_wheel_link_joint_effort = 3;

// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages)
enum class control_mode_type : std::uint8_t
{
  STOP = 0,       // 停止运动
  RUNNING = 1,    // 运动模式
};

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class LqrVmcController : public controller_interface::ControllerInterface
{
public:
  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  LqrVmcController();

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  CallbackReturn on_init() override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration command_interface_configuration() const override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration state_interface_configuration() const override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  LQR_VMC_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  // TODO(anyone): replace the state and command message types
  using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped;      // 应用层控制的消息类型
  using ControllerModeSrvType = std_srvs::srv::SetBool;
  using ControllerStateMsg = nav_msgs::msg::Odometry;    // 控制器发布当前状态的消息类型

protected:
  // 参数设置
  struct params
  {
    std::vector<std::string> joints;
    std::vector<std::string> state_joints;
    std::vector<std::string> command_interfaces;
    std::vector<std::string> state_interfaces;
    double wheel_separation = 0.568;
    double wheel_radius = 0.155;
    double wheel_separation_multiplier = 1.0;
    double left_wheel_radius_multiplier = 1.0;
    double right_wheel_radius_multiplier = 1.0;
    bool tf_frame_prefix_enable = true;
    std::string tf_frame_prefix = "";
    std::string odom_frame_id = "odom";
    std::string base_frame_id = "base_link";
    std::array<double, 6> pose_covariance_diagonal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::array<double, 6> twist_covariance_diagonal = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    double cmd_vel_timeout = 0.5;
    int velocity_rolling_window_size = 10;
    double publish_rate = 50.0;
    std::string sensor_name = "";
    std::string sensor_frame_id = "";
    std::array<double, 9> static_covariance_orientation = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::array<double, 9> static_covariance_angular_velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::array<double, 9> static_covariance_linear_acceleration = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  }params_;

  std::vector<std::string> state_joints_;

  // Command subscribers and Controller State publisher
  bool subscriber_is_active_ = false;
  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
  realtime_tools::RealtimeBox<std::shared_ptr<ControllerReferenceMsg>> received_velocity_msg_ptr_{nullptr};

  rclcpp::Service<ControllerModeSrvType>::SharedPtr set_control_mode_service_;
  realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;

  // IMU话题订阅
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_ = nullptr;
  realtime_tools::RealtimeBox<std::shared_ptr<sensor_msgs::msg::Imu>> received_imu_msg_ptr_{nullptr};

  // 手柄高度调节命令话题订阅
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr height_cmd_subscriber_ = nullptr;
  realtime_tools::RealtimeBox<std::shared_ptr<std_msgs::msg::Bool>> received_height_cmd_msg_ptr_{nullptr};

  // 手柄roll角度调节命令话题订阅
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr roll_cmd_subscriber_ = nullptr;
  realtime_tools::RealtimeBox<std::shared_ptr<std_msgs::msg::Bool>> received_roll_cmd_msg_ptr_{nullptr};

  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;

  // odom_publisher
  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
  std::unique_ptr<ControllerStatePublisher> state_publisher_;

  // tf_publisher
  std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
    nullptr;
  std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
    realtime_odometry_transform_publisher_ = nullptr;

  // 调试需要查看的数据话题发布
  std::shared_ptr<rclcpp::Publisher<debug_msgs::msg::DebugData>> debug_publisher_ = nullptr;
  std::shared_ptr<realtime_tools::RealtimePublisher<debug_msgs::msg::DebugData>>
    realtime_debug_publisher_ = nullptr;

  // 定义odom实例
  Odometry odometry_;

  // 应用控制命令的超时时间
  std::chrono::milliseconds cmd_vel_timeout_{500};
  rclcpp::Time previous_update_timestamp_{0};

  // publish rate limiter
  double publish_rate_ = 50.0;
  rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
  rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};

  // 定义lqr_vmc
  lqr_vmc lqr_vmc_;

  // 控制器更新频率
  double updata_rate_ = 200.0; // 200 Hz
  // 当前Yaw积分
  double current_yaw_inte_{0.0};
  // 目标位移积分和目标Yaw积分
  double displacement_inte_{0.0};
  double yaw_inte_{0.0};
  // 期望大腿角度
  double thigh_angle_d_{8.0*M_PI/180.0};
  // 轨迹跟踪的大腿关节角度
  double thigh_angle_follow_{8.0*M_PI/180.0};
  // 大腿角度规划器
  fivethplanner thighangle_fivethplanner_;
  // 期望roll轴角度
  double roll_angle_d_{0.0};
  // 规划跟踪的roll轴角度
  double roll_angle_follow_{0.0};
  // 手柄控制可倾斜的roll轴角度
  double roll_joy_pos_{5.0*M_PI/180.0};
  // roll轴角度规划器
  fivethplanner rollangle_fivethplanner_;

  // 上次的速度滤波输出值
  double robot_velocity_previous_{0.0};

  // 手柄控制高度的档位
  int now_pos_ = 0; // 当前高度档位
  // 0 1 2 3 4 档位
  std::vector<double> height_joy_pos_{8.0*M_PI/180.0, 13.0*M_PI/180.0, 18.0*M_PI/180.0, 23.0*M_PI/180.0, 28.0*M_PI/180.0};

  // 是否暂停
  bool is_halted = false;
  // 重置
  bool reset();
  // 参数设置回调
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;

private:
  // callback for topic interface
  // 各订阅话题的回调函数
  LQR_VMC_CONTROLLER__VISIBILITY_LOCAL
  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
  LQR_VMC_CONTROLLER__VISIBILITY_LOCAL
  void imu_callback(const std::shared_ptr<sensor_msgs::msg::Imu> imu_msg);
  LQR_VMC_CONTROLLER__VISIBILITY_LOCAL
  void height_cmd_callback(const std::shared_ptr<std_msgs::msg::Bool> height_cmd_msg);
  LQR_VMC_CONTROLLER__VISIBILITY_LOCAL
  void roll_cmd_callback(const std::shared_ptr<std_msgs::msg::Bool> roll_cmd_msg);
};

}  // namespace lqr_vmc_controller

#endif  // LQR_VMC_CONTROLLER__LQR_VMC_CONTROLLER_HPP_
